#include "ColorTracker.h"
#include "../../utilities/ObjPosition.h"

#include <vector>

#define ARRIVING_DIST 1.5 // distance to object (meters) indicating that robot must not correct alignment and should slow down
#define APROACHING_DIST 0.7 // distance to object (meters) indicating that robot is about to stop

#define TURN_SPEED 0.4 // linear speed (mete/second) while turning (in alignment)
#define TIME_FOR_TURN 1000000 // for how long (microseconds) a turn should be published
#define SCALE 3.0 // scales-up turn angles (enables reducing turning time)

#define DISTANCE_TH 1.2 // threshold used to tolerate alignment error according to distance (meters) from object
#define FAR_CENTER_TOL 0.35 // tolerant of alignment error (horizontal deviation in meters) when robot is far
#define CLOSE_CENTER_TOL 0.4 // tolerant of alignment error (horizontal deviation in meters) when robot is close

// average values of each bin in front laser's range that indicate an obstacle
#define OBSTACLE_RIGHT_BIN 0.4
#define OBSTACLE_CENTER_BIN 0.7
#define OBSTACLE_LEFT_BIN 0.4

#define MAX_FRAMES 3 // number of successive frames without detecting the object

#define FIRST_ORDER_SPEED 0.1 // slow-down speed while getting closer to object
#define SECOND_ORDER_SPEED 0.05 // pre-halt speed while getting closer to object (used to stop more accurately)

#define MIN_DIST 0.25 // minimal distance (meters) to an obstacle
#define SUBS_DELAY 2 // delay (seconds) to apply subscriptions

#define TOPIC_NAME "/Sensors_Module/Front_Laser_Range/ColorTracker" // full-name of topic used to receive sensors' data
#define TOPIC_SUFFIX "ColorTracker" // suffix used to identify the color tracker while receiving sensors' data
#define QUEUE_SIZE 5 // ROS topic queue size

enum token_names {
	HORIZONTAL, VERTICAL, DISTANCE, SIZE
}; // index of each token in ROS message which holds object position

ObjPosition obj_pos = ObjPosition(0.0, 0.0, 5.7, 0.0); // working copy of current object position (initialized with default value)

ColorTracker::ColorTracker(Driver* driver, RandomWalk* rand_walk,
		double speed) {

	ROS_INFO("In ColorTracker::ColorTracker");

	m_no_object = 32000; // any big unsigned integer to indicate that no object is detected

	// set driver
	m_driver = driver;
	m_rand_walk = rand_walk;
	m_speed = speed;
	m_backup_speed = speed;

	m_loop_rate = new ros::Rate(6);

	// subscribe to vision system
	m_subscriber = m_node_handler.subscribe("/VisionNode/ObjPosition",
			QUEUE_SIZE, &ColorTracker::positionSetter, this);

	// subscribe to sensors module
	m_sensors_publisher = m_node_handler.advertise<std_msgs::String>(
			"/Sensors_Module/Subscribers_Channel", 10);

	// subscribe to front laser
	sub_sensors_fl = m_node_handler.subscribe(TOPIC_NAME, 1000,
			&ColorTracker::frontLaserCB, this);

	// subscribe to front laser via sensors node
	subscribeToLaser(true);

	// force connection establishment
	m_loop_rate->sleep();
	sleep(1);
}

ColorTracker::~ColorTracker() {

	// stop robot
	m_driver->stop();

	// de-subscribe from sensor
	subscribeToLaser(false);
}

ObjPosition ColorTracker::track(float stop_dist, float recovery_dist) {

	ROS_INFO("ColorTracker::track");

	ObjPosition cur_pos(0.0, 0.0, 9.1, 0.0); // default value
	float prev_dist = 0.0; // distance measured at previous iteration

	m_no_object = 32000; // any big unsigned integer

	int no_obj_ctr = 0; // counts frames with no object- needs to count in order to filter out noise

	while (ros::ok()) {

		ros::spinOnce();

		// set data from previous iteration
		prev_dist = cur_pos.m_dist;
		cur_pos = safeRead();

		// check if object is not visible
		if (cur_pos.m_dist == ERROR_POSITION) {
			ROS_ERROR("NO OBJECT");

			// increment no-object counter
			++no_obj_ctr;

			// find minimal frontal distance
			float right, center, left;
			getBins(right, center, left);

			// random walk check
			if (((right <= OBSTACLE_RIGHT_BIN) || (center <= OBSTACLE_CENTER_BIN) || (left <= OBSTACLE_LEFT_BIN))
					&& no_obj_ctr >= MAX_FRAMES) {

				ROS_INFO("wall- random walk- right = %f, center = %f, left = %f",
						right, center, left);
				m_rand_walk->chooseDirection();
				m_driver->stop();
				sleep(2);
				ros::spinOnce();
			}

			// move straight
			m_driver->move(m_speed, 0);
			m_loop_rate->sleep();
			continue;
		}

		// object is detected
		no_obj_ctr = 0;

		// check if starting to get closer to object
		if (isArrivingToObj(cur_pos.m_dist, prev_dist)) {

			// drive slower in order to stop better
			if (cur_pos.m_dist <= APROACHING_DIST) {
				m_speed = SECOND_ORDER_SPEED;
			} else {
				m_speed = FIRST_ORDER_SPEED;
			}

			// if is not aligned correctly, try again
			if (isWrongAlignment(cur_pos.m_dist)) {

				recovery();
				m_speed = m_backup_speed;
				continue;
			}

			// check if reached object
			if (cur_pos.m_dist <= stop_dist) {

				m_driver->move(-0.2, 0);
				usleep(350000);
				// stop robot
				m_driver->stop();
				ROS_INFO("reached object");
				m_no_object = 0;

				// sample object again and return final position
				sleep(2);
				ros::spinOnce();
				cur_pos = safeRead();

				ROS_INFO("FINAL POSITION = %f,%f,%f", cur_pos.m_x, cur_pos.m_y,
						cur_pos.m_dist);

				m_speed = m_backup_speed;

				// check if should retry
				if (shouldRecover(cur_pos, recovery_dist)) {

					recovery();
					continue;
				}

				// done task
				return cur_pos;
			}

			// drive forward at new speed
			m_driver->move(m_speed, 0);

			m_loop_rate->sleep();
			continue;
		}

		// move towards object
		moveToObject(cur_pos);

		m_loop_rate->sleep();
	}

	return ObjPosition(ERROR_POSITION, ERROR_POSITION, ERROR_POSITION, 0.0);
}

bool ColorTracker::isWrongAlignment(const float& dist) const {
	return dist <= 0;
}

bool ColorTracker::isArrivingToObj(const float& dist, const float& prev_dist) {
	return std::abs(dist) <= ARRIVING_DIST
			&& std::abs(prev_dist) <= ARRIVING_DIST;
}

void ColorTracker::moveToObject(const ObjPosition& cur_pos) {

	// calculate rotation angle according to horizontal position
	float rotateAngle = calcHorizontalOrientation(cur_pos.m_x, cur_pos.m_dist);

	// turn towards object
	if (0 != rotateAngle) {

		// stop and rotate
		ROS_INFO("rotate = %f", rotateAngle);
		m_driver->stop();
		sleep(1);

		m_driver->move(TURN_SPEED, rotateAngle * SCALE);

		usleep(TIME_FOR_TURN);
		m_driver->stop();
		sleep(1);
	}

	// keep moving straight
	ROS_INFO("fixed orientation, driving forward. cur_pos.m_x = %f, cur_pos.m_dist = %f",
			cur_pos.m_x, cur_pos.m_dist);
	m_driver->move(m_speed, 0);
}

float ColorTracker::calcHorizontalOrientation(float xCord, float dist) {

	// ignore false data
	if (dist < 0) {
		return 0;
	}

	// tolerant a small error (according to distance from object)

	if (dist >= DISTANCE_TH && std::abs(xCord) < FAR_CENTER_TOL) {
		return 0;
	}

	if (dist <= DISTANCE_TH &&std::abs(xCord) < CLOSE_CENTER_TOL) {
		return 0;
	}

	// avoid zero division
	if (0 == dist) {
		return 0;
	}

	// return turn angle towards the object
	return std::atan(-xCord / dist);
}

ObjPosition ColorTracker::safeRead() {

	std::unique_lock<std::mutex> mlock(m_mutex);
	ObjPosition cur_pos = obj_pos;
	mlock.unlock();

	return cur_pos;
}

void ColorTracker::positionSetter(const std_msgs::String::ConstPtr& msg) { // callback

	// check error in message
	if (msg->data.empty()) {
		ROS_ERROR("ColorTracker::positionSetter- EMPTY MESSAGE");
		return;
	}

	// parse values out of message
	float x, y;
	float dist;
	float size;
	parseMsg(msg->data, x, y, dist, size);

	// set values of object position
	std::unique_lock<std::mutex> mlock(m_mutex);
	obj_pos.m_x = x;
	obj_pos.m_y = y;
	obj_pos.m_dist = dist;
	obj_pos.m_obj_size = size;
	mlock.unlock();
}

short ColorTracker::getSign(float x) {
	return (x == 0) ? 1 : (short) (x / std::abs(x));
}

void ColorTracker::parseMsg(string msg, float& x, float& y, float& dist,
		float& size) {

	// split message by delimiter
	char dataChars[msg.length()];
	strcpy(dataChars, msg.c_str());

	// store tokens
	vector<string> tokens;
	char* pch = strtok(dataChars, DELIMITER);
	while (pch != NULL) {
		tokens.push_back(pch);
		pch = strtok(NULL, DELIMITER);
	}

	// extract data
//	x = atof(tokens[0].c_str());
//	y = atof(tokens[1].c_str());
//	dist = atof(tokens[2].c_str());
//	size = atof(tokens[3].c_str());
	x = atof(tokens[HORIZONTAL].c_str());
	y = atof(tokens[VERTICAL].c_str());
	dist = atof(tokens[DISTANCE].c_str());
	size = atof(tokens[SIZE].c_str());
}

/*
 * Publishes to sensor topic - subscribes to message from
 * sensors when the distance is reached
 */

void ColorTracker::subscribeToLaser(bool shouldSubscribe) {

	// compose message
	std_msgs::String m_message_to_publish;
	char buffer[128];
	snprintf(buffer, sizeof(buffer), "%s%s%d%s%d%s%f%s%f",
	TOPIC_SUFFIX, DELIMITER, sensors_names::FRONT_LASER_RANGE, DELIMITER,
			int(shouldSubscribe), DELIMITER, 30.0, DELIMITER, 720.0);
	m_message_to_publish.data = buffer;

	// publish the message to sensors
	m_sensors_publisher.publish(m_message_to_publish);
	sleep(SUBS_DELAY);
}

void ColorTracker::frontLaserCB(
		const std_msgs::Float32MultiArray::ConstPtr& range) {

	if (range->data.size() <= 0) {
		ROS_ERROR("ColorTracker::frontLaserCB data size = %d",
				range->data.size());
		return;
	}

	// calculate average distance for each bin
	float right = avgElements(range, 180, 200);
	float center = avgElements(range, 350, 370);
	float left = avgElements(range, 520, 540);

	// set the values safely
	setBins(right, center, left);

	// set all the range safely
	setRange(range);

	if (m_no_object != 0) {
		isEmergencyStop();
	}
}

bool ColorTracker::isEmergencyStop() {

	// find minimal frontal distance
	float right, center, left;
	getBins(right, center, left);

	// check each direction
	if ((right <= MIN_DIST) || (center <= MIN_DIST) || (left <= MIN_DIST)) {

		ROS_ERROR("ColorTracker:: emergency stop");

		m_driver->stop();
		usleep(500000);
		return true;
	}

	return false;
}

float ColorTracker::avgElements(
		const std_msgs::Float32MultiArray::ConstPtr& range, const int& start,
		const int& end) const {

	// sum elements
	float sum = 0.0;
	for (int i = start; i < end; ++i) {
		sum += range->data[i];
	}

	// return average
	return sum / (end - start + 1);
}

float ColorTracker::avgElements(const float range[], const int& start,
		const int& end) const {

	// sum elements
	float sum = 0.0;
	for (int i = start; i < end; ++i) {
		sum += range[i];
	}

	// return average
	return sum / (end - start + 1);
}

void ColorTracker::getBins(float& right, float& center, float& left) {

	std::unique_lock<std::mutex> mlock(m_mutex);
	right = m_right_bin;
	center = m_center_bin;
	left = m_left_bin;
	mlock.unlock();
}

void ColorTracker::setBins(const float right, const float center,
		const float left) {

	std::unique_lock<std::mutex> mlock(m_mutex);
	m_right_bin = right;
	m_center_bin = center;
	m_left_bin = left;
	mlock.unlock();
}

void ColorTracker::setRange(
		const std_msgs::Float32MultiArray::ConstPtr& range) {

	std::unique_lock<std::mutex> mlock(m_mutex);
	for (int i = 0; i < MAX_RANGE_SIZE; ++i) {
		m_range[i] = range->data[i];
	}
	mlock.unlock();
}

void ColorTracker::sleepAndWatch(int time_for_turn) { /*in microseconds*/

	for (int i = 0; i < 1000; ++i) {
		ros::spinOnce();
		usleep(time_for_turn / 1000);
	}
}

void ColorTracker::recovery() {

	m_driver->move(-0.3, 0);
	sleepAndWatch(1000000);

	// turn 180 degrees
	m_driver->move(0, 2.5);
	m_no_object = 32000;
	sleepAndWatch(2000000);

	// drive forward
	m_driver->move(0.3, 0);
	sleepAndWatch(6000000);

	// turn 180 degrees
	m_driver->move(0, 2.0);
	sleepAndWatch(2000000);
	m_driver->move(0, -0.15);
}

bool ColorTracker::shouldRecover(const ObjPosition& final_pos, const float& recovery_dist) const {

	// if not intending to catch object, no need to run recovery
	ROS_INFO("ColorTracker::shouldRecover: rec_dist = %f, dist = %f", recovery_dist, final_pos.m_dist);
	if (recovery_dist > (float) ARM_RANGE) {
		ROS_ERROR("no recovery");
		return false;
	}

	// trying to catch object- run recovery if final position is invalid or too far
	return (final_pos.m_dist <= 0 || final_pos.m_dist >= recovery_dist);
}
